#!/usr/bin/env python
#-*- coding:UTF-8 -*-
import rospy
import Jetson.GPIO as GPIO
import time

# 设置默认针脚序号
beep_ch =31
but_ch =7
led_ch1=11
led_ch2=13
led_ch3=15
led_ch4=29

# 舵机控制函数
def gpio_servo_ctrl(channel, dutycycle=0.10):
    # 输出脉冲信号模拟pwm，用于控制舵机
    # channel为针脚标号，已测试33，35，37可用
    # dutycycle为占空比，控制舵机目标角度

    GPIO.setmode(GPIO.BOARD)        # 设置Jetson nano板正面对应针脚标号
    GPIO.setup(channel, GPIO.OUT)   # 设置输出通道
    GPIO.setwarnings(True)          

    TIME=0.018                               # 周期20ms
    high_time = TIME*dutycycle              # 高电平时长
    low_time = TIME*(1-dutycycle)           # 低电平时长

    for i in range(20):                      # 循环输出
        GPIO.output(channel, GPIO.HIGH) 
        time.sleep(high_time)
        GPIO.output(channel, GPIO.LOW)
        time.sleep(low_time)
        time.sleep(0.001)                   # 周期间隔

# 按钮控制，等待按下后蜂鸣器提醒
def gpio_button_ctrl(channel):
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(channel, GPIO.IN)
    
    GPIO.wait_for_edge(channel, GPIO.BOTH)
    gpio_beep_ctrl(beep_ch)

    GPIO.cleanup()

# 蜂鸣器控制函数
def gpio_beep_ctrl(channel, ctime=0.5):

    GPIO.setmode(GPIO.BOARD)        # 设置Jetson nano板正面对应针脚标号
    GPIO.setup(channel, GPIO.OUT)   # 设置输出通道

    GPIO.output(channel, GPIO.HIGH) 
    time.sleep(ctime)
    GPIO.output(channel, GPIO.LOW)
    time.sleep(ctime)
    GPIO.output(channel, GPIO.HIGH)
    time.sleep(ctime)
    GPIO.output(channel, GPIO.LOW)


#LED亮灯
def gpio_led_on(channel):
    GPIO.setmode(GPIO.BOARD)        # 设置Jetson nano板正面对应针脚标号
    GPIO.setup(channel, GPIO.OUT)   # 设置输出通道

    GPIO.output(channel, GPIO.LOW) 

#LED灭灯
def gpio_led_off(channel):
    GPIO.setmode(GPIO.BOARD)        # 设置Jetson nano板正面对应针脚标号
    GPIO.setup(channel, GPIO.OUT)   # 设置输出通道

    GPIO.output(channel, GPIO.HIGH) 


 

